/********************************************
* Copyrigt (C) rumi
* Module: serv_datapack.c
* History: Created on 2021.8.10
* Version:1.0.0 add ros2 support
* Version:1.0.1 fix comservice recive and send
*********************************************/
#include "serv_datapack.h"
/********************************************/
uint16_t CRC16_Check_Code(uint8_t * data, uint16_t length)
{
    uint16_t i, crc_reg, CrcData;

    crc_reg = 0xFFFF;

    while(length--)
    {
        CrcData = *data++;
        crc_reg ^= CrcData;

        for(i = 0; i < 8; i++)
        {
            if(crc_reg & 0x01)
            {
                crc_reg = (crc_reg >> 1) ^ 0xA001;
            }
            else
            {
                crc_reg = (crc_reg >> 1);
            }
        }
    }
    return crc_reg;		
}

/************************************************************************
* CBuff_Init
************************************************************************/
void CBuff_Init(CBuff *buffer, uint8_t *pBuff, uint32_t size)
{
    buffer->pBuff = pBuff;
    buffer->Size = size;
    buffer->Head = 0;
    buffer->Tail = 0;
}

/************************************************************************
* CBuff_GetLength
************************************************************************/
uint32_t CBuff_GetLength(const CBuff *buffer)
{
    return (buffer->Head >= buffer->Tail) ? (buffer->Head - buffer->Tail) : (buffer->Size + buffer->Head - buffer->Tail);
}

/************************************************************************
* CBuff_Write
************************************************************************/
bool CBuff_Write(CBuff *buffer, const uint8_t *data, uint32_t len)
{
    uint32_t freeSpace = buffer->Size - CBuff_GetLength(buffer) - 1; 
    uint32_t Head,i;

    if (freeSpace < len)
    {
        return false; 
    }

    Head = buffer->Head;
    for (i = 0; i < len; i++)
    {
        buffer->pBuff[Head] = data[i];
        Head = (Head + 1) % buffer->Size;
    }
    buffer->Head = Head;

    return true; 
}
/************************************************************************
* CBuff_Read
************************************************************************/
bool CBuff_Read(CBuff *buffer, uint8_t *data, uint32_t len)
{
    uint32_t availableData = CBuff_GetLength(buffer);
    uint32_t Tail,i;

    if (availableData < len)
    {
        return false; 
    }
    
    Tail = buffer->Tail;
    for (i = 0; i < len; i++)
    {
        data[i] = buffer->pBuff[Tail];
		Tail = (Tail + 1) % buffer->Size;
    }
    
    return true; 
}
/************************************************************************
* CBuff_Pop
************************************************************************/
bool CBuff_Pop(CBuff *buffer, uint8_t *data, uint32_t len)
{
    uint32_t availableData = CBuff_GetLength(buffer);
    uint32_t Tail,i;

    if (availableData < len)
    {
        return false; 
    }
    
    Tail = buffer->Tail;
    for (i = 0; i < len; i++)
    {
        data[i] = buffer->pBuff[Tail];
        Tail = (Tail + 1) % buffer->Size;
    }
    buffer->Tail = Tail;
    return true; 
}

/************************************************************************
* CBuff_Clear
************************************************************************/
void CBuff_Clear(CBuff *buffer)
{
    buffer->Head = 0;
    buffer->Tail = 0;
}

/************************************************************************
* CBuff_IsEmpty
************************************************************************/
bool CBuff_IsEmpty(const CBuff *buffer)
{
    return (buffer->Head == buffer->Tail);
}

/************************************************************************
* CBuff_IsFull
************************************************************************/
bool CBuff_IsFull(const CBuff *buffer)
{
    return ((buffer->Head + 1) % buffer->Size) == buffer->Tail;
}

/************************************************************************
* CBuff_GetFreeSpace
************************************************************************/
uint32_t CBuff_GetFreeSpace(const CBuff *buffer)
{
    return buffer->Size - CBuff_GetLength(buffer) - 1; 
}


uint8_t* Serv_Com_PackageData(COM_FRAME_HEAD_ENUM FrameHead,COM_FRAME_CRC_ENUM FrameTail,uint8_t *Buffer,uint8_t DataLen)
{
    uint8_t lbOut = true;
    static uint8_t TmpSendBuff[64];
    uint8_t SendCount=0;
    COM_SEND_STATE_ENUM Send_State = E_COMSENSENDSTATE_INIT;
    while(lbOut)
    {
        switch(Send_State)
        {
            case E_COMSENSENDSTATE_INIT:
            {    
                SendCount = 0;
                Send_State = E_COMSENSENDSTAT_FRAMEHEAD;      
                break;
            }

            case E_COMSENSENDSTAT_FRAMEHEAD:
            {
                switch (FrameHead)
                {
                    case E_COMFRAMEHEAD_NONE:
                    {
                        TmpSendBuff[SendCount++]=DEVICE_HEAD;
                        TmpSendBuff[SendCount++]=0x00;
                        TmpSendBuff[SendCount++]=0; // Length Byte
                        break;
                    }

                    case E_COMFRAMEHEAD_NORMAL:
                    {
                        TmpSendBuff[SendCount++]=DEVICE_HEAD;
                        TmpSendBuff[SendCount++]=0xA0;
                        TmpSendBuff[SendCount++]=0; // Length Byte                       
                        break;
                    }
                    default:
                        break; 
                }
                Send_State = E_COMSENSENDSTAT_DATA;                          
                break;
            }
            
            case E_COMSENSENDSTAT_DATA:
            {
                for (uint8_t i = 0; i < DataLen; i++)
                {
                    TmpSendBuff[SendCount++] = Buffer[i];
                }
                Send_State = E_COMSENSENDSTAT_FRAMEEND;               
                break;
            }

            case E_COMSENSENDSTAT_FRAMEEND:
            {
                switch (FrameTail)
                {
                    case E_COMFRAMETAIL_NONE:
                    {
                        TmpSendBuff[1] += 0;
                        break;
                    }
                    case E_COMFRAMETAIL_JustFloat:
                    {
                        TmpSendBuff[1] += 1;
                        TmpSendBuff[SendCount++] = 0x00;
                        TmpSendBuff[SendCount++] = 0x00;
                        TmpSendBuff[SendCount++] = 0x80;
                        TmpSendBuff[SendCount++] = 0x7F;
                        break;
                    }
                    case E_COMFRAMETAIL_CRC16:
                    {
                        TmpSendBuff[1] += 5;
                        uint16_t u16CRC = CRC16_Check_Code(Buffer,DataLen);
                        TmpSendBuff[SendCount++] = (u16CRC >> 8) & 0xFF;
                        TmpSendBuff[SendCount++] = u16CRC & 0xFF;
                        break;
                    }                    
                    default:
                        break; 
                }
                Send_State = E_COMSENSENDSTAT_LENGTH;          
                break;
            }

            case E_COMSENSENDSTAT_LENGTH:
            {
                TmpSendBuff[PackDataLen] = SendCount;
                lbOut = false; 
                break;
            }

            default:
            {
                lbOut = false;
                break;
            }
        }
    }
    return TmpSendBuff;
}
/********************************************/
// run in 5ms handle
uint8_t Serv_Com_UnpackData(CBuff *lRecive_CBuff, uint8_t *sucReciveData)
{
    uint16_t u16CRC_Sum=0,u16CRC_Calc=0xFFFF;
	uint8_t Cbuff_Len;
    uint8_t ret = 0;
      
    Cbuff_Len = CBuff_GetLength(lRecive_CBuff);
    if(Cbuff_Len < 6)
    {
		return ret;
	}
    CBuff_Read(lRecive_CBuff,sucReciveData,4);
    if(sucReciveData[0] != TARGET_HEAD)
	{
		CBuff_Pop(lRecive_CBuff,sucReciveData,1);	// if not TARGET_HEAD Delete the first data
		return ret;
	}
    else
    {
        lRecive_CBuff->HeadType = E_COMFRAMEHEAD_NONE;   
        lRecive_CBuff->TailType = E_COMFRAMETAIL_NONE;
        lRecive_CBuff->HandleDataLength = 0;
        switch(sucReciveData[1])
        {
            case 0xA0:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_NORMAL;lRecive_CBuff->TailType = E_COMFRAMETAIL_NONE; break;
            case 0xA1:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_NORMAL;lRecive_CBuff->TailType = E_COMFRAMETAIL_JustFloat;break;
            case 0xA5:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_NORMAL;lRecive_CBuff->TailType = E_COMFRAMETAIL_CRC16;break;           
            case 0xB0:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_UPDATE;lRecive_CBuff->TailType = E_COMFRAMETAIL_NONE; break;          
            case 0xB5:lRecive_CBuff->HeadType = E_COMFRAMEHEAD_UPDATE;lRecive_CBuff->TailType = E_COMFRAMETAIL_CRC16;break;
            default:break;
        }
        // must have one frame head
        if(lRecive_CBuff->HeadType == E_COMFRAMEHEAD_NONE)
        {
            CBuff_Pop(lRecive_CBuff,sucReciveData,1);
            return ret;
        }
        lRecive_CBuff->HandleDataLength = sucReciveData[PackDataLen];
        if (lRecive_CBuff->HandleDataLength > 64)		
        {		
            CBuff_Pop(lRecive_CBuff,sucReciveData,2); 	// detect if the length over the range
            return ret;
        }
        else if(CBuff_GetLength(lRecive_CBuff) < lRecive_CBuff->HandleDataLength)
        {
            lRecive_CBuff->HandleDataOverTime++;
            if(lRecive_CBuff->HandleDataOverTime >= 20)   // 5ms*20 = 100ms
            {
                lRecive_CBuff->HandleDataOverTime = 0;
                CBuff_Pop(lRecive_CBuff,sucReciveData,2);				// 100ms don't receive the rest of the data, remove header and return
            }
            return ret;
        }
        else
        {
            lRecive_CBuff->HandleDataOverTime = 0;
            CBuff_Read(lRecive_CBuff,sucReciveData,lRecive_CBuff->HandleDataLength);			// Pull out all the data
            switch(lRecive_CBuff->TailType)
            {
                case E_COMFRAMETAIL_NONE:
                {
                    u16CRC_Calc = 0xfefe;
                    u16CRC_Sum = 0xfefe;
                    break;
                }
                case E_COMFRAMETAIL_CRC16:
                {
                    u16CRC_Calc = CRC16_Check_Code(sucReciveData+3,(lRecive_CBuff->HandleDataLength-5)); 
                    u16CRC_Sum = sucReciveData[lRecive_CBuff->HandleDataLength-2]*256 + sucReciveData[lRecive_CBuff->HandleDataLength-1];
                    break;
                }
                default:break;
            }
            	
            if(u16CRC_Calc != u16CRC_Sum)
            {
                CBuff_Pop(lRecive_CBuff,sucReciveData,2); 					// CRC error,Delete the head
            }
            else
            {
                CBuff_Pop(lRecive_CBuff,sucReciveData,lRecive_CBuff->HandleDataLength);
                ret = 1;
                return ret;                   
            }
        }
    }
    return ret;
}

/********************************************
 * End of file
*********************************************/
